#include<formic/lbfgs.h>
//#include<formic/test_driver/utilities.h>

class TestWorker {

  private:

    std::vector<double> params;
    std::vector<double> v;
    double _gn;

    double pre_sqr(const double * const x) {
      double retval = 0.0;
      for (int i = 0; i < 3; i++) {
        retval += params.at(3*i+0) * ( x[2*i+0] - v[2*i+0] ) * ( x[2*i+0] - v[2*i+0] );
        retval += params.at(3*i+1) * ( x[2*i+0] - v[2*i+0] ) * ( x[2*i+1] - v[2*i+1] );
        retval += params.at(3*i+2) * ( x[2*i+1] - v[2*i+1] ) * ( x[2*i+1] - v[2*i+1] );
      }
      return retval;
    }

  public:

    TestWorker() {

      params.push_back(1.2);
      params.push_back(0.6);
      params.push_back(0.2);
      params.push_back(2.2);
      params.push_back(0.1);
      params.push_back(1.3);
      params.push_back(0.3);
      params.push_back(1.7);
      params.push_back(6.2);

      v.push_back(0.7);
      v.push_back(0.2);
      v.push_back(0.4);
      v.push_back(0.9);
      v.push_back(1.1);
      v.push_back(0.5);

      _gn = 1.0e+100;

    }

    void get_value(const int n, const double * const x, double & val) {
      const double pre_sq_val = this->pre_sqr(x);
      val = pre_sq_val * pre_sq_val;
    }

    void get_gradient(const int n, const double * const x, double * const g) {

      for (int i = 0; i < 3; i++) {

        g[2*i+0] =   params.at(3*i+0) * 2.0 * ( x[2*i+0] - v[2*i+0] )
                   + params.at(3*i+1) * 1.0 * ( x[2*i+1] - v[2*i+1] );

        g[2*i+1] =   params.at(3*i+1) * 1.0 * ( x[2*i+0] - v[2*i+0] )
                   + params.at(3*i+2) * 2.0 * ( x[2*i+1] - v[2*i+1] );

      }

      const double pre_sq_val = 2.0 * this->pre_sqr(x);
      for (int i = 0; i < n; i++)
        g[i] *= pre_sq_val;

      _gn = 0.0;
      for (int i = 0; i < n; i++)
        _gn += g[i] * g[i];
      _gn = std::abs( std::sqrt(_gn) );

    }

    void print(const int n, const std::string & message, const double * const x) {
      formic::of << message << std::endl;
      for (int i = 0; i < n; i++)
        formic::of << boost::format("%20.12f") % x[i] << std::endl;
      formic::of << std::endl;
    }

    void process_new_point(const int n, const double * const x) {

    }

    double most_recent_grad_norm() { return _gn; }

};

int main(int argc, char ** argv) {

  formic::mpi::init(argc, argv);

  // open the output file
  formic::open_output_file("output.txt");

  //formic::set_seed(std::time(NULL));
  //formic::LaggedFibonacci<> lfg;

  const int n = 6;

  std::vector<double> x0(n, 0.0);
  std::vector<double> x1(n, 0.0);

  x0.at(0) = 0.729;
  x0.at(1) = 0.191;
  x0.at(2) = 0.397;
  x0.at(3) = 0.912;
  x0.at(4) = 1.091;
  x0.at(5) = 0.511;

  const double thresh = 1.0e-10;

  TestWorker tw;
  formic::lbfgs(tw, 100, 3, n, thresh, 0.0, &x0.at(0), &x1.at(0));

  if ( tw.most_recent_grad_norm() > thresh )
    throw formic::Exception("final lbfgs gradient norm was %.2e, above the threshold of %.2e") % tw.most_recent_grad_norm() % thresh;

  formic::close_output_file();

  formic::mpi::finalize();

  return EXIT_SUCCESS;

}
